#include <math.h>
#include <unistd.h>
#include <pthread.h>

#include "lcm/lcm.h"

#include "common/getopt.h"
#include "common/timestamp.h"
#include "lcmtypes/diff_drive_t.h"
#include "lcmtypes/difteleop_t.h"
#include "lcmtypes/simplepose_t.h"
#include "lcmtypes/teleop_t.h"
#include "lcmtypes/waypointlist_t.h"
#include "lcmtypes/motor_feedback_t.h"

#include "LinAlg.h"
#include "Line.h"

#define BASELINE 0.184

// will eventually need to extend this to describe the pose of the robot as well
typedef struct Pose_struct {
	// odometry measurements in clicks [left right]
	int32_t lastClicksLR[2];
	int32_t deltaClicksLR[2];

	// timing information
	int64_t lastUtime;
	int64_t deltaUtime;

	//Need to keep track of pose x,y,theta these are world coordinates
	float x;
	float y;
	float theta;

	//Need to keep track of linear and angular velocities - may want a circle buffer of deltas to average over
	float speed;
	float dxdt;
	float dydt;
	float dthetadt;
} Pose;

float cmdLRT[3]; 	// left and right motor commands set by the teleop listener thread and run by the diffdrive publisher thread
PolyLine *path;
Pose *pose;
pthread_mutex_t cmdLRTlock;
pthread_mutex_t poselock;
pthread_mutex_t pathlock;

/* Program for the pandabot to listen in for differential drive commands and execute them directly*/
/* stealing guts out of diff_publisher.c example updates the cmdLR values with the commanded value*/
/*static void handler_teleop(const lcm_recv_buf_t *rbuf, const char *channel,
                              const difteleop_t *msg, void *user)
{
	printf("Got teleop\n");
	float *cmdLR = (float *) user;
	// left 
	cmdLR[0] = msg->left;
	// right
	cmdLR[1] = msg->right;
}*/

// constructs differential drive commands from given speed and turn commands
static void handler_teleop(const lcm_recv_buf_t *rbuf, const char *channel,
                              const teleop_t *msg, void *user)
{
	printf("got a message!\n");
	// cmdLRT is a global variable, don't need to pass it in
	float dT = msg->dtheta;
	float s = msg->speed; // speed is between 0 and 100%

	// calculate ratio of left to right to achieve desired angular velocity
	float dL = (2*s - BASELINE*dT)/2;
	float dR = 2*s - dL;

	float sf = 1; 
	//compute scale factor to normalize so that largest command is 1
	if (fabsf(dL) >= 1) {
		sf = 1/fabsf(dL);
	} else if (fabsf(dR) >=1 ) {
		sf = 1/fabsf(dR);
	}

	// scale results
	dR = sf*dR;
	dL = sf*dL;

	// left 
	pthread_mutex_lock(&cmdLRTlock);
	cmdLRT[0] = dL;
	// right
	cmdLRT[1] = dR;
	// the last component is a counter to give us a time out period
	cmdLRT[2] = 0;
	pthread_mutex_unlock(&cmdLRTlock);
}

static void handler_waypoints(const lcm_recv_buf_t *rbuf, const char *channel,
                              const waypointlist_t *msg, void *user) 
{
	printf("got waypoints!\n");
	pthread_mutex_lock(&pathlock);

	printf("we locked!\n");

	// path is the polyline
	// destroy all the things
	if (path != NULL) {
		//TODO there is a bug in Polyline destroy that I am trakcing down, why don't you work on setting up the 
		// PID controller with the anticipation that polyline destroy will be fixed
		printf("freeing verticies");
		int i;
		// destroy all the points
		for (i = 0; i < path->vertices->size; i++) {
			Vec_destroy(path->vertices->list[i]);
		}
		printf("destroying polyline\n");
		//VecList_destroyR(path->vertices);
		PolyLine_destroy(path);
		printf("we destroyed\n");
	}

	// reinitialize
	
	int i;
	float p[] = {0.0, 0.0};
	VecList *vlist = VecList_createS(msg->number);
	for (i = 0; i < msg->number; i++) {
		p[0] = msg->x[i];
		p[1] = msg->y[i];
		VecList_add(vlist,Vec_create(p,2));
	}

	// recreate our polyline
	printf("initializign polyline\n");
	path = PolyLine_init(vlist);
	printf("poly line created!\n");
	pthread_mutex_unlock(&pathlock);
	printf("unlocked!\n");

	// clean up, init copies the vlist
	VecList_destroy(vlist);
}

static void handler_motor_feedback(const lcm_recv_buf_t *rbuf, const char *channel,
                              const motor_feedback_t *msg, void *user)
{

	//printf("Got motor feedback\n");
	//printf("\tLeft: %d \tRight: %d\n", msg->encoders[0], msg->encoders[1]);

	// how much time elapsed since our last encoder reading
	int64_t curtime = timestamp_now();
	pthread_mutex_lock(&poselock);
	pose->deltaUtime = curtime - pose->lastUtime;
	pose->lastUtime = curtime;

	// how much have our encoders changed by
	pose->deltaClicksLR[0] = msg->encoders[0] - pose->lastClicksLR[0]; 
	pose->deltaClicksLR[1] = msg->encoders[1] - pose->lastClicksLR[1];
	pose->lastClicksLR[0] = msg->encoders[0];
	pose->lastClicksLR[1] = msg->encoders[1];

	// generate pose interms of x, y, theta
	float clickperm = 3.021077e-04; 	// meters per click
	float baseline = 0.184;			// baseline distance between the wheels

	float dL = clickperm * pose->deltaClicksLR[0];
	float dR = clickperm * pose->deltaClicksLR[1];

	float d = (dL + dR)/2;			// forwards movement
	float dtheta = (dR - dL)/baseline;  	// change in heading

	float dl[] = {d, 0, 1};
	Vec	*pl = Vec_create(dl,3);
	Matrix *rbtl = Matrix_createRBT2D(0,0,dtheta);						// spits out dx and dy in the local frame
	Matrix *rbt_ltog = Matrix_createRBT2D(pose->x,pose->y,pose->theta); // transforms local to global coordinates

	// this line is a memory leak since we aren't capturing the intermediate vector that is being created!!
	Vec *pg = Matrix_multiplyAV(rbt_ltog,Matrix_multiplyAV(rbtl,pl)); // global x y coordinates

	// generate linear and angular velocities
	float dt = (pose->deltaUtime)*10e-6;			// calculate elapsed time
	pose->speed = d/dt;
	pose->dxdt = (pg->val[0] - pose->x)/dt;
	pose->dydt = (pg->val[1] - pose->y)/dt;
	pose->dthetadt = dtheta/dt;

	// store our new pose information
	pose->x = pg->val[0];
	pose->y = pg->val[1];
	pose->theta += dtheta;

	pthread_mutex_unlock(&poselock);

	// clean up
	Vec_destroy(pl);
	Vec_destroy(pg);
	Matrix_destroy(rbtl);
	Matrix_destroy(rbt_ltog);
}

//LCM handler threads
void listenerThread(void *arg)
{
	lcm_t *listener = (lcm_t*) arg;
	while(1)
	{
		if (lcm_handle(listener))
		{
		    exit(1);
		}
	}
}

//LCM Publisher threads
void diffdrivepublisherthread(void *arg) 
{
  	lcm_t *channel = (lcm_t*) arg;
	printf("did that work: %f, %f, %f \n",cmdLRT[0],cmdLRT[1],cmdLRT[2]);
	while(TRUE) {
		usleep(100000);
		while(cmdLRT[2] < 10)
		{
			usleep(100000);
			// lock
			pthread_mutex_lock(&cmdLRTlock);
			diff_drive_t cmd = {.left = cmdLRT[0], .right = cmdLRT[1], .left_enabled = 1, .right_enabled = 1};
			(cmdLRT[2])++; // increment but the teleop listener thread which also has this will change it back to 0
			pthread_mutex_unlock(&cmdLRTlock);
			cmd.utime = timestamp_now();	

			diff_drive_t_publish(channel,"Team10_DIFF_DRIVE", &cmd);
		}
	}
}

// PID controller thread
// PolyLine *path
// pthread_mutex_t poselock;
// pthread_mutex_t pathlock;
void PIDthread(void *arg) {
	printf("staring PID thread\n");

	lcm_t *channel = (lcm_t*) arg;
	
	Vec *heading, *pos, *perp, *Verror;
	Line *curL;
	float Etheta, Espeed, Linetheta, EdistP, Etheta2, commandTheta, commandSpeed;

	// set points for our PID controller
	float thetaSet, speedSet; 
	
	while(TRUE) {
		usleep(10000);
		// lock
		pthread_mutex_lock(&poselock);
		pthread_mutex_lock(&pathlock);

		// grab the line
		curL = path->Llist->list[path->pos];
	
		// convert orientation to a heading vector
		float dydxR = tan(pose->theta);
		float headingA[] = {1, dydxR, 0};
		heading = Vec_create(headingA,3);
		Vec_normalizeI(heading);

		
		// compute heading error
		Linetheta = atan2(curL->direction->val[1],curL->direction->val[0]);
		Etheta = pose->theta - Linetheta;

		// distance error from line
		float posA[] = {pose->x, pose->y, 0}; 
		pos = Vec_create(posA,3);
		perp = Line_perpendicular(curL,pos);
		Verror = Vec_crossProduct(heading,Verror);
		EdistP = Vec_L2norm(perp)*(Verror->val[2])/fabsf(Verror->val[2]);

		// compute setpoints from 
		thetaSet = -Etheta - atan(EdistP);
		speedSet = 0.5;

		// compute errors
		Etheta2 = thetaSet - pose->theta;
		Espeed = speedSet - pose->speed;

		// P control
		commandTheta = 1 * Etheta2;
		commandSpeed = 1 * Espeed;

		teleop_t cmd = {.speed = commandSpeed, .dtheta = commandTheta};
		cmd.utime = timestamp_now();	
		teleop_t_publish(channel,"Team10_TELLEOP", &cmd);

		// unlock
		pthread_mutex_unlock(&poselock);
		pthread_mutex_unlock(&poselock);

		// clean up
		Vec_destroy(heading);
		Vec_destroy(pos);
		Vec_destroy(perp);
		Vec_destroy(Verror);
	}
}


int main(int argc, char *argv[])
{
	// lcm objects
	lcm_t *lcmLocal = lcm_create(NULL); //"udpm://239.255.76.67:7667?ttl=0")
	lcm_t *lcmGlobal = lcm_create("udpm://239.255.76.10:7667?ttl=1");

	// most recent command
	cmdLRT[0] = 0.0;
	cmdLRT[1] = 0.0;
	cmdLRT[2] = 0.0;
	pose->lastClicksLR[0] =0;
	pose->lastClicksLR[1] = 0;
	pose->deltaClicksLR[0] = 0;
	pose->deltaClicksLR[0] = 0;
	pose->lastUtime = timestamp_now();
	pose->deltaUtime = 0;

	// subscribe to the teleoperation messages you don't need the return value
	waypointlist_t_subscribe(lcmGlobal, "Team10_WAYPOINTS", handler_waypoints, NULL);
	teleop_t_subscribe(lcmLocal, "Team10_TELEOP", handler_teleop, NULL);
	motor_feedback_t_subscribe(lcmLocal, "MOTOR_FEEDBACK", handler_motor_feedback, NULL);

	//initialize mutex locks
	pthread_mutex_init(&cmdLRTlock, NULL);
	pthread_mutex_init(&poselock, NULL);

	//Start listener threads
	pthread_t *listenerLocal, *listenerGlobal;
	pthread_create(&listenerLocal, NULL, listenerThread, lcmLocal);
	pthread_create(&listenerGlobal, NULL, listenerThread, lcmGlobal);

	// start diff drive thread to publish diff drive commands
	pthread_t *diffDrive, *pidcontrol;
	pthread_create(&diffDrive, NULL, diffdrivepublisherthread, lcmLocal);
	pthread_create(&pidcontrol, NULL, PIDthread,  lcmLocal);

	usleep(100000); // wait a bit to allow the initial clicks to update

	int cntr = 0;
	while(1) {
		usleep(100000);
		pthread_mutex_lock(&poselock);
		simplepose_t pubPose = {.pos = {pose->x, pose->y}, .vel = {pose->dxdt, pose->dydt}, .orientation = pose->theta, .rotation_rate = pose->dthetadt, .accel = {0, 0}};
		pthread_mutex_unlock(&poselock);
		simplepose_t_publish(lcmGlobal, "Team10_POSE", &pubPose);

		cntr++;
	}

	lcm_destroy(lcmLocal);
	lcm_destroy(lcmGlobal);

	return 0;
}
